
_SENSORS_init:

;sensors.c,13 :: 		void SENSORS_init() {
;sensors.c,14 :: 		I2C1_Init(400000);
	MOVLW       30
	MOVWF       SSPADD+0 
	CALL        _I2C1_Init+0, 0
;sensors.c,15 :: 		Delay_ms(500);
	MOVLW       31
	MOVWF       R11, 0
	MOVLW       113
	MOVWF       R12, 0
	MOVLW       30
	MOVWF       R13, 0
L_SENSORS_init0:
	DECFSZ      R13, 1, 1
	BRA         L_SENSORS_init0
	DECFSZ      R12, 1, 1
	BRA         L_SENSORS_init0
	DECFSZ      R11, 1, 1
	BRA         L_SENSORS_init0
	NOP
;sensors.c,17 :: 		ITG3200_init();
	CALL        _ITG3200_init+0, 0
;sensors.c,18 :: 		Delay_ms(500);
	MOVLW       31
	MOVWF       R11, 0
	MOVLW       113
	MOVWF       R12, 0
	MOVLW       30
	MOVWF       R13, 0
L_SENSORS_init1:
	DECFSZ      R13, 1, 1
	BRA         L_SENSORS_init1
	DECFSZ      R12, 1, 1
	BRA         L_SENSORS_init1
	DECFSZ      R11, 1, 1
	BRA         L_SENSORS_init1
	NOP
;sensors.c,19 :: 		BMA180_init();
	CALL        _BMA180_init+0, 0
;sensors.c,20 :: 		Delay_ms(500);
	MOVLW       31
	MOVWF       R11, 0
	MOVLW       113
	MOVWF       R12, 0
	MOVLW       30
	MOVWF       R13, 0
L_SENSORS_init2:
	DECFSZ      R13, 1, 1
	BRA         L_SENSORS_init2
	DECFSZ      R12, 1, 1
	BRA         L_SENSORS_init2
	DECFSZ      R11, 1, 1
	BRA         L_SENSORS_init2
	NOP
;sensors.c,21 :: 		}
	RETURN      0
; end of _SENSORS_init

_SENSORS_read:

;sensors.c,23 :: 		void SENSORS_read(float *SENSORS_acc, float *SENSORS_gyro) {
;sensors.c,24 :: 		BMA180_readAcc(&SENSORS_acc);
	MOVLW       FARG_SENSORS_read_SENSORS_acc+0
	MOVWF       FARG_BMA180_readAcc_acc+0 
	MOVLW       hi_addr(FARG_SENSORS_read_SENSORS_acc+0)
	MOVWF       FARG_BMA180_readAcc_acc+1 
	CALL        _BMA180_readAcc+0, 0
;sensors.c,25 :: 		ITG3200_readGyro(&SENSORS_gyro);
	MOVLW       FARG_SENSORS_read_SENSORS_gyro+0
	MOVWF       FARG_ITG3200_readGyro_gyro+0 
	MOVLW       hi_addr(FARG_SENSORS_read_SENSORS_gyro+0)
	MOVWF       FARG_ITG3200_readGyro_gyro+1 
	CALL        _ITG3200_readGyro+0, 0
;sensors.c,26 :: 		}
	RETURN      0
; end of _SENSORS_read

_SENSORS_process:

;sensors.c,28 :: 		void SENSORS_process(float *SENSORS_acc, float *SENSORS_gyro, float *SENSORS_angle) {
;sensors.c,31 :: 		vector = sqrt(SENSORS_acc[0]*SENSORS_acc[0] + SENSORS_acc[1]*SENSORS_acc[1] + SENSORS_acc[2]*SENSORS_acc[2]);
	MOVFF       FARG_SENSORS_process_SENSORS_acc+0, FSR0L
	MOVFF       FARG_SENSORS_process_SENSORS_acc+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVF        R0, 0 
	MOVWF       R4 
	MOVF        R1, 0 
	MOVWF       R5 
	MOVF        R2, 0 
	MOVWF       R6 
	MOVF        R3, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__SENSORS_process+0 
	MOVF        R1, 0 
	MOVWF       FLOC__SENSORS_process+1 
	MOVF        R2, 0 
	MOVWF       FLOC__SENSORS_process+2 
	MOVF        R3, 0 
	MOVWF       FLOC__SENSORS_process+3 
	MOVLW       4
	ADDWF       FARG_SENSORS_process_SENSORS_acc+0, 0 
	MOVWF       R0 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_acc+1, 0 
	MOVWF       R1 
	MOVLW       4
	ADDWF       FARG_SENSORS_process_SENSORS_acc+0, 0 
	MOVWF       FSR2L 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_acc+1, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R4 
	MOVF        POSTINC2+0, 0 
	MOVWF       R5 
	MOVF        POSTINC2+0, 0 
	MOVWF       R6 
	MOVF        POSTINC2+0, 0 
	MOVWF       R7 
	MOVFF       R0, FSR0L
	MOVFF       R1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__SENSORS_process+0, 0 
	MOVWF       R4 
	MOVF        FLOC__SENSORS_process+1, 0 
	MOVWF       R5 
	MOVF        FLOC__SENSORS_process+2, 0 
	MOVWF       R6 
	MOVF        FLOC__SENSORS_process+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__SENSORS_process+0 
	MOVF        R1, 0 
	MOVWF       FLOC__SENSORS_process+1 
	MOVF        R2, 0 
	MOVWF       FLOC__SENSORS_process+2 
	MOVF        R3, 0 
	MOVWF       FLOC__SENSORS_process+3 
	MOVLW       8
	ADDWF       FARG_SENSORS_process_SENSORS_acc+0, 0 
	MOVWF       R0 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_acc+1, 0 
	MOVWF       R1 
	MOVLW       8
	ADDWF       FARG_SENSORS_process_SENSORS_acc+0, 0 
	MOVWF       FSR2L 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_acc+1, 0 
	MOVWF       FSR2H 
	MOVF        POSTINC2+0, 0 
	MOVWF       R4 
	MOVF        POSTINC2+0, 0 
	MOVWF       R5 
	MOVF        POSTINC2+0, 0 
	MOVWF       R6 
	MOVF        POSTINC2+0, 0 
	MOVWF       R7 
	MOVFF       R0, FSR0L
	MOVFF       R1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__SENSORS_process+0, 0 
	MOVWF       R4 
	MOVF        FLOC__SENSORS_process+1, 0 
	MOVWF       R5 
	MOVF        FLOC__SENSORS_process+2, 0 
	MOVWF       R6 
	MOVF        FLOC__SENSORS_process+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_sqrt_x+0 
	MOVF        R1, 0 
	MOVWF       FARG_sqrt_x+1 
	MOVF        R2, 0 
	MOVWF       FARG_sqrt_x+2 
	MOVF        R3, 0 
	MOVWF       FARG_sqrt_x+3 
	CALL        _sqrt+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__SENSORS_process+4 
	MOVF        R1, 0 
	MOVWF       FLOC__SENSORS_process+5 
	MOVF        R2, 0 
	MOVWF       FLOC__SENSORS_process+6 
	MOVF        R3, 0 
	MOVWF       FLOC__SENSORS_process+7 
	MOVF        FLOC__SENSORS_process+4, 0 
	MOVWF       SENSORS_process_vector_L0+0 
	MOVF        FLOC__SENSORS_process+5, 0 
	MOVWF       SENSORS_process_vector_L0+1 
	MOVF        FLOC__SENSORS_process+6, 0 
	MOVWF       SENSORS_process_vector_L0+2 
	MOVF        FLOC__SENSORS_process+7, 0 
	MOVWF       SENSORS_process_vector_L0+3 
;sensors.c,32 :: 		SENSORS_angle[0] = SENSORS_complementaryFilter(SENSORS_angle[0], asin(SENSORS_acc[1]/vector)*57.295779, -SENSORS_gyro[0]);
	MOVF        FARG_SENSORS_process_SENSORS_angle+0, 0 
	MOVWF       FLOC__SENSORS_process+0 
	MOVF        FARG_SENSORS_process_SENSORS_angle+1, 0 
	MOVWF       FLOC__SENSORS_process+1 
	MOVLW       4
	ADDWF       FARG_SENSORS_process_SENSORS_acc+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_acc+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVF        FLOC__SENSORS_process+4, 0 
	MOVWF       R4 
	MOVF        FLOC__SENSORS_process+5, 0 
	MOVWF       R5 
	MOVF        FLOC__SENSORS_process+6, 0 
	MOVWF       R6 
	MOVF        FLOC__SENSORS_process+7, 0 
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_asin_x+0 
	MOVF        R1, 0 
	MOVWF       FARG_asin_x+1 
	MOVF        R2, 0 
	MOVWF       FARG_asin_x+2 
	MOVF        R3, 0 
	MOVWF       FARG_asin_x+3 
	CALL        _asin+0, 0
	MOVLW       225
	MOVWF       R4 
	MOVLW       46
	MOVWF       R5 
	MOVLW       101
	MOVWF       R6 
	MOVLW       132
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newAngle+0 
	MOVF        R1, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newAngle+1 
	MOVF        R2, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newAngle+2 
	MOVF        R3, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newAngle+3 
	MOVFF       FARG_SENSORS_process_SENSORS_angle+0, FSR0L
	MOVFF       FARG_SENSORS_process_SENSORS_angle+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_oldAngle+0 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_oldAngle+1 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_oldAngle+2 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_oldAngle+3 
	MOVFF       FARG_SENSORS_process_SENSORS_gyro+0, FSR0L
	MOVFF       FARG_SENSORS_process_SENSORS_gyro+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       0
	XORWF       R0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newRate+0 
	MOVLW       0
	XORWF       R1, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newRate+1 
	MOVLW       128
	XORWF       R2, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newRate+2 
	MOVLW       0
	XORWF       R3, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newRate+3 
	CALL        _SENSORS_complementaryFilter+0, 0
	MOVFF       FLOC__SENSORS_process+0, FSR1L
	MOVFF       FLOC__SENSORS_process+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
;sensors.c,33 :: 		SENSORS_angle[1] = SENSORS_complementaryFilter(SENSORS_angle[1], asin(SENSORS_acc[0]/vector)*57.295779, SENSORS_gyro[1]);
	MOVLW       4
	ADDWF       FARG_SENSORS_process_SENSORS_angle+0, 0 
	MOVWF       FLOC__SENSORS_process+0 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_angle+1, 0 
	MOVWF       FLOC__SENSORS_process+1 
	MOVFF       FARG_SENSORS_process_SENSORS_acc+0, FSR0L
	MOVFF       FARG_SENSORS_process_SENSORS_acc+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVF        SENSORS_process_vector_L0+0, 0 
	MOVWF       R4 
	MOVF        SENSORS_process_vector_L0+1, 0 
	MOVWF       R5 
	MOVF        SENSORS_process_vector_L0+2, 0 
	MOVWF       R6 
	MOVF        SENSORS_process_vector_L0+3, 0 
	MOVWF       R7 
	CALL        _Div_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_asin_x+0 
	MOVF        R1, 0 
	MOVWF       FARG_asin_x+1 
	MOVF        R2, 0 
	MOVWF       FARG_asin_x+2 
	MOVF        R3, 0 
	MOVWF       FARG_asin_x+3 
	CALL        _asin+0, 0
	MOVLW       225
	MOVWF       R4 
	MOVLW       46
	MOVWF       R5 
	MOVLW       101
	MOVWF       R6 
	MOVLW       132
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newAngle+0 
	MOVF        R1, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newAngle+1 
	MOVF        R2, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newAngle+2 
	MOVF        R3, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newAngle+3 
	MOVLW       4
	ADDWF       FARG_SENSORS_process_SENSORS_angle+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_angle+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_oldAngle+0 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_oldAngle+1 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_oldAngle+2 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_oldAngle+3 
	MOVLW       4
	ADDWF       FARG_SENSORS_process_SENSORS_gyro+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_gyro+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newRate+0 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newRate+1 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newRate+2 
	MOVF        POSTINC0+0, 0 
	MOVWF       FARG_SENSORS_complementaryFilter_newRate+3 
	CALL        _SENSORS_complementaryFilter+0, 0
	MOVFF       FLOC__SENSORS_process+0, FSR1L
	MOVFF       FLOC__SENSORS_process+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
;sensors.c,34 :: 		SENSORS_angle[2] += (SENSORS_gyro[2]*SENSORS_processTime);
	MOVLW       8
	ADDWF       FARG_SENSORS_process_SENSORS_angle+0, 0 
	MOVWF       FLOC__SENSORS_process+0 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_angle+1, 0 
	MOVWF       FLOC__SENSORS_process+1 
	MOVLW       8
	ADDWF       FARG_SENSORS_process_SENSORS_gyro+0, 0 
	MOVWF       FSR0L 
	MOVLW       0
	ADDWFC      FARG_SENSORS_process_SENSORS_gyro+1, 0 
	MOVWF       FSR0H 
	MOVF        POSTINC0+0, 0 
	MOVWF       R0 
	MOVF        POSTINC0+0, 0 
	MOVWF       R1 
	MOVF        POSTINC0+0, 0 
	MOVWF       R2 
	MOVF        POSTINC0+0, 0 
	MOVWF       R3 
	MOVLW       143
	MOVWF       R4 
	MOVLW       194
	MOVWF       R5 
	MOVLW       117
	MOVWF       R6 
	MOVLW       120
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVFF       FLOC__SENSORS_process+0, FSR0L
	MOVFF       FLOC__SENSORS_process+1, FSR0H
	MOVF        POSTINC0+0, 0 
	MOVWF       R4 
	MOVF        POSTINC0+0, 0 
	MOVWF       R5 
	MOVF        POSTINC0+0, 0 
	MOVWF       R6 
	MOVF        POSTINC0+0, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVFF       FLOC__SENSORS_process+0, FSR1L
	MOVFF       FLOC__SENSORS_process+1, FSR1H
	MOVF        R0, 0 
	MOVWF       POSTINC1+0 
	MOVF        R1, 0 
	MOVWF       POSTINC1+0 
	MOVF        R2, 0 
	MOVWF       POSTINC1+0 
	MOVF        R3, 0 
	MOVWF       POSTINC1+0 
;sensors.c,35 :: 		}
	RETURN      0
; end of _SENSORS_process

_SENSORS_complementaryFilter:

;sensors.c,37 :: 		float SENSORS_complementaryFilter(float oldAngle, float newAngle, float newRate) {
;sensors.c,42 :: 		filteredAngle = filterK* (oldAngle + newRate * SENSORS_processTime) + (1-filterK) * (newAngle);
	MOVF        FARG_SENSORS_complementaryFilter_newRate+0, 0 
	MOVWF       R0 
	MOVF        FARG_SENSORS_complementaryFilter_newRate+1, 0 
	MOVWF       R1 
	MOVF        FARG_SENSORS_complementaryFilter_newRate+2, 0 
	MOVWF       R2 
	MOVF        FARG_SENSORS_complementaryFilter_newRate+3, 0 
	MOVWF       R3 
	MOVLW       143
	MOVWF       R4 
	MOVLW       194
	MOVWF       R5 
	MOVLW       117
	MOVWF       R6 
	MOVLW       120
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FARG_SENSORS_complementaryFilter_oldAngle+0, 0 
	MOVWF       R4 
	MOVF        FARG_SENSORS_complementaryFilter_oldAngle+1, 0 
	MOVWF       R5 
	MOVF        FARG_SENSORS_complementaryFilter_oldAngle+2, 0 
	MOVWF       R6 
	MOVF        FARG_SENSORS_complementaryFilter_oldAngle+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
	MOVLW       79
	MOVWF       R4 
	MOVLW       236
	MOVWF       R5 
	MOVLW       68
	MOVWF       R6 
	MOVLW       126
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        R0, 0 
	MOVWF       FLOC__SENSORS_complementaryFilter+0 
	MOVF        R1, 0 
	MOVWF       FLOC__SENSORS_complementaryFilter+1 
	MOVF        R2, 0 
	MOVWF       FLOC__SENSORS_complementaryFilter+2 
	MOVF        R3, 0 
	MOVWF       FLOC__SENSORS_complementaryFilter+3 
	MOVLW       196
	MOVWF       R0 
	MOVLW       78
	MOVWF       R1 
	MOVLW       108
	MOVWF       R2 
	MOVLW       124
	MOVWF       R3 
	MOVF        FARG_SENSORS_complementaryFilter_newAngle+0, 0 
	MOVWF       R4 
	MOVF        FARG_SENSORS_complementaryFilter_newAngle+1, 0 
	MOVWF       R5 
	MOVF        FARG_SENSORS_complementaryFilter_newAngle+2, 0 
	MOVWF       R6 
	MOVF        FARG_SENSORS_complementaryFilter_newAngle+3, 0 
	MOVWF       R7 
	CALL        _Mul_32x32_FP+0, 0
	MOVF        FLOC__SENSORS_complementaryFilter+0, 0 
	MOVWF       R4 
	MOVF        FLOC__SENSORS_complementaryFilter+1, 0 
	MOVWF       R5 
	MOVF        FLOC__SENSORS_complementaryFilter+2, 0 
	MOVWF       R6 
	MOVF        FLOC__SENSORS_complementaryFilter+3, 0 
	MOVWF       R7 
	CALL        _Add_32x32_FP+0, 0
;sensors.c,43 :: 		return filteredAngle;
;sensors.c,44 :: 		}
	RETURN      0
; end of _SENSORS_complementaryFilter
